package robocup.component.worldmodel.particlefilter;

import java.util.Random;

import robocup.component.SConf;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.BallInfo;

/**
 * Particle filter used for improving the balls position and speed.
 * 
 * (based on the UvA-Trilearn Master Thesis)
 * 
 * @author werwurm
 * 
 */
public class BallParticleFilter {

  private final Vektor           particlesPos[] = Vektor.getDummyInstances(BallParticleFilter.particleCount);

  private final Vektor           particlesSpeed[] = Vektor.getDummyInstances(BallParticleFilter.particleCount);

  private final Vektor           particlesPos2[] = Vektor.getDummyInstances(BallParticleFilter.particleCount);

  private static final int particleCount          = 300;

  private boolean          speedInitialized       = false;

  private boolean          ballKicked             = true;

  private int              validParticles         = 0;

  private final Random           random                 = new Random(System
                                                      .currentTimeMillis());

  private final BallRegion       posRegionHelper        = new BallRegion();

  private final BallSpeedRegion  speedRegionHelper      = new BallSpeedRegion();

  private final Vektor           vektorHelper           = new Vektor();

  private int              translationSinceLastVI = 0;

  /**
   * This class is a particle-filter for estimating the balls position and
   * velocity.
   */
  public BallParticleFilter() {

    super();

    this.validParticles = 1;
  }

  public void resetPosition(final Vektor pos,
      final Vektor speed) {

    if (pos == null) {
      this.validParticles = 0;
    }
    else {
      this.validParticles = 1;
      this.particlesPos[0].x = pos.x;
      this.particlesPos[0].y = pos.y;
      this.particlesSpeed[0].x = speed.x;
      this.particlesSpeed[0].y = speed.y;

    }
    this.translationSinceLastVI = 0;
  }

  public void addVI(final BallInfo bi,
      final double headAngle,
      final Vektor myPos,
      final Vektor mySpeed) {

    boolean speedSeen = bi.velocitySeen();
    this.posRegionHelper.reinitRegionPos(bi, headAngle, myPos);
    if (speedSeen) {
      this.speedRegionHelper.reinitRegion(bi, headAngle, this.posRegionHelper,
          mySpeed);
    }

    if (this.validParticles != 0) {
      this.ballKicked = false;
      if (!this.speedInitialized) {
        if (speedSeen) {
          for (int i = 0; i < this.validParticles; i++) {
            this.speedRegionHelper
                .makeRandomInsideRegion(this.particlesSpeed[i]);
          }
          this.speedInitialized = true;
        }
        else if (this.translationSinceLastVI == 1) {
          for (int i = 0; i < this.particlesPos2.length; i++) {
            this.posRegionHelper.makeRandomInsideRegion(this.particlesPos2[i]);
            final Vektor chosen = this.particlesPos[this.random
                .nextInt(this.validParticles)];
            this.particlesSpeed[i].x = this.particlesPos2[i].x;
            this.particlesSpeed[i].y = this.particlesPos2[i].y;
            this.particlesSpeed[i].subFromThis(chosen);
          }
          
          // swap particlesPos
          for(int i=0; i<this.particlesPos.length; i++){
            final double x = particlesPos[i].x;
            particlesPos[i].x = particlesPos2[i].x;
            particlesPos2[i].x = x;
            
            final double y = particlesPos[i].y;
            particlesPos[i].y = particlesPos2[i].y;
            particlesPos2[i].y = y;
          }
          
          this.validParticles = this.particlesPos.length;
        }
        this.speedInitialized = true;
      }
      int lastValidPlusOne = 0;
      for (int i = 0; i < this.validParticles; i++) {
        if (this.posRegionHelper.insideRegion(this.particlesPos[i]) == 1 && (!this.speedInitialized || !speedSeen || this.speedRegionHelper
            .insideRegion(this.particlesSpeed[i]) == 1)) {
          vektorHelper.copy(this.particlesPos[i]);
          this.particlesPos[i].copy(this.particlesPos[lastValidPlusOne]);
          this.particlesPos[lastValidPlusOne].copy(vektorHelper);
          
          vektorHelper.copy(this.particlesSpeed[i]);
          this.particlesSpeed[i].copy(this.particlesSpeed[lastValidPlusOne]);
          this.particlesSpeed[lastValidPlusOne].copy(vektorHelper);
          
          lastValidPlusOne++;
        }
      }
      this.validParticles = lastValidPlusOne;
      if (lastValidPlusOne != 0) {
        // clone valid Particles
        for (int i = lastValidPlusOne; i < this.particlesPos.length; i++) {
          final int chosen = this.random.nextInt(lastValidPlusOne);
          this.particlesPos[i].x = this.particlesPos[chosen].x;
          this.particlesPos[i].y = this.particlesPos[chosen].y;
          this.particlesSpeed[i].x = this.particlesSpeed[chosen].x;
          this.particlesSpeed[i].y = this.particlesSpeed[chosen].y;
        }
        this.validParticles = this.particlesPos.length;
      }
    }

    if (this.validParticles == 0) {
      this.ballKicked = true;
      for (int i = 0; i < this.particlesPos.length; i++) {
        this.posRegionHelper.makeRandomInsideRegion(this.particlesPos[i]);
        if (speedSeen) {
          this.speedRegionHelper.makeRandomInsideRegion(this.particlesSpeed[i]);
        }
      }
      this.speedInitialized = speedSeen;
      this.validParticles = this.particlesPos.length;
    }
    this.translationSinceLastVI = 0;
  }

  public boolean isSpeedInitialized() {

    return this.speedInitialized;
  }

  public Vektor getPosition() {

    if (this.validParticles == 0) {
      return null;
    }
    final Vektor mean = this.vektorHelper;
    mean.x = 0;
    mean.y = 0;
    for (int i = 0; i < this.validParticles; i++) {
      mean.x += this.particlesPos[i].x;
      mean.y += this.particlesPos[i].y;
    }
    mean.x /= (double) this.validParticles;
    mean.y /= (double) this.validParticles;
    return mean.cloned();
  }

  public Vektor getSpeed() {

    if (this.validParticles == 0 || !this.speedInitialized) {
      return null;
    }
    final Vektor mean = this.vektorHelper;
    mean.x = 0;
    mean.y = 0;
    for (int i = 0; i < this.validParticles; i++) {
      mean.x += this.particlesSpeed[i].x;
      mean.y += this.particlesSpeed[i].y;
    }
    mean.x /= (double) this.validParticles;
    mean.y /= (double) this.validParticles;
    return mean.cloned();

  }

  public boolean isBallKicked() {

    return this.ballKicked;
  }

  public void translatePosition() {

    this.translationSinceLastVI++;
    if (!this.speedInitialized) {
      return;
    }
    for (int i = 0; i < this.validParticles; i++) {
      final double rmax = SConf.getInstance().BALL_RAND * this.particlesSpeed[i]
          .getLength();
      this.vektorHelper.x = this.particlesSpeed[i].x + ((this.random
          .nextDouble() * 2.0) - 1.0) * rmax;
      this.vektorHelper.y = this.particlesSpeed[i].y + ((this.random
          .nextDouble() * 2.0) - 1.0) * rmax;
      this.particlesPos[i].addToThis(this.vektorHelper);
      this.particlesSpeed[i].x = this.vektorHelper.x * SConf.getInstance().BALL_DECAY;
      this.particlesSpeed[i].y = this.vektorHelper.y * SConf.getInstance().BALL_DECAY;
    }
  }
}
